/* Copyright (c) 2010 Fabian Kulman
* 
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
* 
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
* 
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/

#include <fstream>
#include <algorithm>

#include "chain.h"
#include "gui.h"

using namespace std;


#include <iostream>
Chain::Chain( cpSpace* space, vector<cpFloat>* v ):vUnit(NULL),space(space),vInput(v){
		nrUnits = vInput->size() + 1;


		// Initialize units
		cpVect pos = { 0.0, 0.0 };
		vUnit.push_back( new Unit( space, pos ) );

		Unit* u = NULL;		// new unit
		Unit* lu = NULL;	// left unit

		for(int i=1; i<nrUnits; ++i){
			lu = vUnit[i-1]; 
			u = new Unit(space, lu, (*vInput)[i-1]);
			vUnit.push_back( u );
			lu->ru = u;					
		}
	}


	Unit* Chain::pointInChain( cpVect p ){
		Unit* unit = NULL;
		cpShape* shape = cpSpacePointQueryFirst( space, p, GRABABLE_MASK_BIT, 0 );

		if(shape){
			cpHashValue id = shape->hashid;

			for(int i=0; i<nrUnits; ++i){
				if( id == vUnit[i]->id){
					unit = vUnit[i];
				}
			}
		}
		return unit;
	}


	/*vector<cpFloat>* Chain::boundingBox(){

		vector<cpFloat>* minMaxValues = new vector<cpFloat>();
		vector<cpFloat> xValues;
		vector<cpFloat> yValues;

		cpVect* verts;
			for(int i=0; i<nrUnits; ++i){
				verts = ((cpPolyShape*)vUnit[i]->shape)->tVerts;	
				for(int j=0; j<4; ++j){
					xValues.push_back(verts[j].x);
					yValues.push_back(verts[j].y);
				}	
			}

			minMaxValues->push_back( *min_element(xValues.begin(),xValues.end()) );
			minMaxValues->push_back( *max_element(xValues.begin(),xValues.end()) );
			minMaxValues->push_back( *min_element(yValues.begin(),yValues.end()) );
			minMaxValues->push_back( *max_element(yValues.begin(),yValues.end()) );
		
			return minMaxValues;
	}*/

	/*cpVect Chain::calculateCenter(){

		vector<cpFloat>* minMaxValues = boundingBox(); 

		cpFloat xMin = (*minMaxValues)[0];
		cpFloat xMax = (*minMaxValues)[1];
		cpFloat yMin = (*minMaxValues)[2];
		cpFloat yMax = (*minMaxValues)[3];

		return cpv((xMin+xMax)/2.0,(yMin+yMax)/2.0);
	}*/


	void Chain::startDragRotation( Unit* u ){
		u->unlookJoint();

		rit = vUnit.rbegin(); 
		while( u->lu->id != (*rit)->id ){
			(*rit)->unlookPosition();
			rit++;
		}
	}


	void Chain::stopDragRotation( Unit* u ){
		u->lookJoint();

		rit = vUnit.rbegin(); 
		while( u->lu->id != (*rit)->id ){
			(*rit)->lookPosition();
			rit++;
		}
	}

	void Chain::stopChain(){
		for(int i=0; i<nrUnits; ++i){
			vUnit[i]->stopUnit();
		}
	}


	void Chain::printChainState(){
		ofstream file("print.fr");
		{
			for( int i=1; i<nrUnits; ++i){
				file << vUnit[i]->state << std::endl;
			}
		}
		file.close();
	}


	void Chain::drawChain( GLfloat guiScale ){
		for( int i=0; i< this->nrUnits; i++){
			this->vUnit[i]->drawUnit( guiScale );
		}
	}

	Chain::~Chain(){
		for(int i=0; i<nrUnits; ++i){
			Unit* u = vUnit[i];
			delete u;
		}
	}

	//void Chain::snapRotation( Unit* u ){

	//	cpVect* tVerts = ((cpPolyShape*)u->shape)->tVerts;
	//	cpVect impulse = cpvforangle(u->body->a + 1.5708); 
	//	cpFloat force = 500.0;

	//	if( u->state == 1.0){
	//		impulse = cpvmult(impulse, -force);
	//	}
	//	else if( u->state == 0.0 ){
	//		impulse = cpvmult(impulse, force);
	//	}
	//	else{
	//		impulse = cpvmult(impulse, force);
	//	}

	//	cout << impulse.x << endl;
	//	cout << impulse.y << endl;
	//	

	//	rit = vUnit.rbegin(); // last element
	//
	//	while( u->lu->id != (*rit)->id ){
	//		cpBodyApplyImpulse((*rit)->body, impulse, (*rit)->body->p );
	//		rit++;
	//	}
	//}

